/******************************************************************************
 * Copyright Authors DustinKsi@ Youibot 2019. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include "gtest/gtest_prod.h"
#include "venus/mapping/interface/abstract_mapping.h"

#include <map>
#include <queue>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include <cmath>
#include <sstream>
#include <string>

#include "venus/mapping/carto_mapping/feature_pair_cost.h"

#include <ceres/ceres.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <opencv2/opencv.hpp>

using std::unordered_set;
using std::unordered_map;
using std::set;
using std::map;
using std::pair;
using std::make_pair;
using std::priority_queue;
using std::queue;
using std::vector;

using std::max;
using std::min;
using std::stringstream;

namespace VEnus {
namespace Mapping {
class CartoMapping : public AbstractMapping {
 private:
  int feature_point_cnt;
  unordered_map<int, pair<double, double> > feature_points;
  unordered_map<int, unordered_map<int, double> > feature_graph;

 public:
  ~CartoMapping();
  bool Init();

  bool InsertFeatureList(VEnus::Sensor::Feature2DList &feature_list,
                         VEnus::Sensor::AdjacencyList &adjacency_list,
                         VEnus::Sensor::RobotPose &robot_pose);

  void OptimizeCurrentPose(vector<pair<pair<double, double>, pair<double, double> > > &match_result,
                           VEnus::Sensor::RobotPose &robot_pose);

  bool ComputeCurrentPose(vector<pair<pair<double, double>, pair<double, double> > > &match_result,
                          VEnus::Sensor::RobotPose &robot_pose);

  bool CheckResultValid(vector<double> &radius_list,
                        vector<pair<pair<double, double>, pair<double, double> > > &match_result,
                        pair<double, double> &result, int id1, int id2, double d12);

  void SiftNewFeaturePoints(vector<pair<double, double> > &input_fpts,
                            vector<pair<int, int> > &hit_id_pair,
                            vector<pair<double, double> > &feature_points_wait_insert);

  inline double ComputeDistance(pair<double, double> &pt1, pair<double, double> &pt2);

  inline double ComputeRadius(pair<double, double> &pt);

  void InsertToFeatureGraph(vector<pair<double, double> > &feature_points_wait_insert,
                            vector<int> &feature_points_assigned_id);
  
  bool SetInitialPose(VEnus::Sensor::RobotPose &pose);

 private:
  friend class MappingFactory;
  FRIEND_TEST(CartoMappingTest, SiftNewFeaturePoints);
  FRIEND_TEST(CartoMappingTest, FakeMapBuild);

  CartoMapping();
};

}  // namespace Mapping

}  // namespace VEnus
